package frc.circuitrunners.Archive;
import edu.wpi.first.wpilibj.SpeedController;
/**
 * Tank Drive
 * @author James
 */
public class TankDrive extends AbstractDrive {
    private static int LEFT, RIGHT;
    public TankDrive(SpeedController[] mCs){
        if(mCs.length != 2 || mCs.length != 4){
            throw new IllegalArgumentException("Length of motor controllers array inccorect. Expected: 2 or 4. Given: "+mCs.length);
        }
        motorControllers = mCs;
    }
    /**
     * 
     * @param left - how fast the left side of the robot should be moving, needs to be between -1.0 and 1.0, or it will be truncated to the aformentioned limits
     * @param right - how fast the left side of the robot should be moving, needs to be between -1.0 and 1.0, or it will be truncated to the aformentioned limits 
     */
    public void drive(double left, double right){
        left = limit(left);   //limit the input coming in
        right = limit(right);
        if(motorControllers.length == 2) { // In the case of 2 motors
            motorControllers[RIGHT].set(right); //Set right 
            motorControllers[LEFT].set(left);   //Set left
        } else if (motorControllers.length == 4){ // In the case of 4 motors
            motorControllers[FRONT_RIGHT].set(right); //set front right
            motorControllers[BACK_RIGHT].set(right);  //set back right
            motorControllers[FRONT_LEFT].set(left);   //set front left
            motorControllers[BACK_LEFT].set(left);    //set back left
        } else { //In the case that there are and odd number of motors
            throw new IllegalArgumentException("Incorrect number of indecies, Array has "+motorControllers.length+" number of indecies.");
        }
    }
    
    public void setLeft(int index){
        if(motorControllers.length == 4){
            throw new IllegalArgumentException("Incorrect Usage of Method: wrong number of drive motors, use setBackLeft() or setFrontLeft instead");
        }
        if(index < 0 || index > motorControllers.length){
            throw new IllegalArgumentException("Length of motor controllers array inccorect. Expected: Between 0 and "
                    + motorControllers.length + ". Given: "+index);
        }
        LEFT = index;
    }   
    public void setRight(int index){
        if(motorControllers.length == 4){
            throw new IllegalArgumentException("Incorrect Usage of Method: wrong number of drive motors, use setBackRight() or setFrontRight() instead");
        }
        if(index < 0 || index > motorControllers.length){
            throw new IllegalArgumentException("Length of motor controllers array inccorect. Expected: Between 0 and "
                    + motorControllers.length + ". Given: "+index);
        }
        RIGHT = index;
    }
}
